////////////////////////////////////////////////////////////////////////////////

#ifndef _PHYSICSENGINE_H_
#include "physicsengine.h"
#endif

////////////////////////////////////////////////////////////////////////////////

#ifndef _PHYSICALOBJECT_H_
#include "physicalobject.h"
#endif

#ifndef _MATHFUNCTIONS_H_
#include "mathfunctions.h"
#endif

////////////////////////////////////////////////////////////////////////////////

namespace PE
{
////////////////////////////////////////////////////////////////////////////////


PhysicsEngine::PhysicsEngine()
{
}


////////////////////////////////////////////////////////////////////////////////


PhysicsEngine::~PhysicsEngine()
{
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::AddObject( PhysicalObject* _object )
{
    m_Objects.insert(_object);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::DeleteObject( PhysicalObject* _object )
{
    m_Objects.erase(_object);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::Update( size_t _dt )
{
    // Update objects positions
    for (ObjectsIter it = m_Objects.begin(); it != m_Objects.end(); it++)
        UpdatePosition(*it, _dt);

    // Process collisions
    ProcessCollisions();
}


////////////////////////////////////////////////////////////////////////////////


bool PhysicsEngine::IsIntersects( PhysicalObject* _obj1,
                                  PhysicalObject* _obj2 ) const
{
    // Get object's positions
    Point p1 = _obj1->GetPosition();
    Point p2 = _obj2->GetPosition();

    // Minimal allowed distance between two objects
    double minDist = _obj1->GetRadius() + _obj2->GetRadius();

    // Current distance between two objects
    double curDist = Distance(p1, p2);

    // Check for intersects of two objects
    return curDist < minDist;
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::UpdatePosition( PhysicalObject* _obj, size_t _dt )
{
    // Get object's current position and velocity
    Vector vel = _obj->GetVelocity();
    Point pos = _obj->GetPosition();
    
    // Calculate new position
    Point newPos(pos.x + vel.vx * _dt, pos.y + vel.vy * _dt);
    _obj->SetPosition(newPos);

    // Calculate new speed
    double resistance = _obj->GetResistanceCoefficient();
    if ( resistance )
    {
        double resistanceForce = VectorLength(vel)*resistance;
        _obj->ApplyImpulse(GetDirection(-vel), resistanceForce);
    }
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::ProcessCollisions()
{
    for (ObjectsIter objIt1 = m_Objects.begin(); objIt1 != m_Objects.end(); objIt1++)
    {
        ObjectsIter objIt2 = objIt1;
        while ( ++objIt2 != m_Objects.end() )
        {
            // Check for intersection of two objects
            if ( IsIntersects(*objIt1, *objIt2) )
            {
                Point p1 = (*objIt1)->GetPosition();
                Point p2 = (*objIt2)->GetPosition();

                // Get collision normal vector
                Vector normal = {p1.x - p2.x, p1.y - p2.y};

                // Get impulse direction
                Direction impulseDir = GetDirection(normal);

                // Get impulse value
                double impulse = (*objIt1)->GetMass() * VectorLength( (*objIt1)->GetVelocity() ) +
                                 (*objIt2)->GetMass() * VectorLength( (*objIt2)->GetVelocity() );

                // Apply impulses
                (*objIt1)->ApplyImpulse(impulseDir, impulse);
                (*objIt2)->ApplyImpulse(impulseDir, -impulse);

                // Inform objects about collision
                (*objIt1)->Collision(*objIt2);
                (*objIt2)->Collision(*objIt1);
            }
        }
    }
}


////////////////////////////////////////////////////////////////////////////////

}; // namespace PE

////////////////////////////////////////////////////////////////////////////////
